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Abstract:  Several  approaches  for  incorporating  nav¬ 
igation  function  approach  into  different  controllers  are 
developed  in  this  paper  for  task  execution  by  a  non- 
holonomic  system  (e.g.,  a  wheeled  mobile  robot)  in  the 
presence  of  known  obstacles.  The  first  approach  is  a 
path  planning-based  control  with  planning  a  desired 
path  based  on  a  3-dimensional  position  and  orienta¬ 
tion  information.  A  navigation-like  function  yields  a 
path  from  an  initial  configuration  inside  the  free  con¬ 
figuration  space  of  the  mobile  robot  to  a  goal  config¬ 
uration.  A  differentiable,  oscillator-based  controller  is 
then  used  to  enable  the  mobile  robot  to  follow  the  path 
and  stop  at  the  goal  position.  A  second  approach  is 
developed  for  a  navigation  function  that  is  constructed 
using  2-dimensional  position  information.  A  differen¬ 
tiable  controller  is  proposed  based  on  this  navigation 
function  that  yields  asymptotic  convergence.  Simula¬ 
tion  results  are  provided  to  illustrate  the  performance 
of  the  second  approach. 

1  Introduction 

Numerous  researchers  have  proposed  algorithms  to  ad¬ 
dress  the  motion  control  problem  associated  with  ro¬ 
botic  task  execution  in  an  obstacle  cluttered  environ¬ 
ment.  A  comprehensive  summary  of  techniques  that 
address  the  classic  geometric  problem  of  constructing 
a  collision-free  path  and  traditional  path  planning  al¬ 
gorithms  is  provided  in  Section  9,  “Literature  Land¬ 
marks”,  of  Chapter  1  of  [19].  Since  the  pioneering  work 
by  Khatib  in  [13],  it  is  clear  that  the  construction  and 
use  of  potential  functions  has  continued  to  be  one  of 
the  mainstream  approaches  to  robotic  task  execution 
among  known  obstacles.  In  short,  potential  functions 
produce  a  repulsive  potential  field  around  the  robot 
workspace  boundary  and  obstacles  and  an  attractive 
potential  field  at  the  goal  configuration.  A  comprehen¬ 
sive  overview  of  research  directed  at  potential  functions 
is  provided  in  [19].  One  of  criticisms  of  the  potential 
function  approach  is  that  local  minima  can  occur  that 
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0589,  and  a  DARPA  Contract  at  Clemson  University,  and  in  part 
by  AFOSR  contract  number  F49620-03-1-0381  at  the  University 
of  Florida. 


can  cause  the  robot  to  “get  stuck”  without  reaching  the 
goal  position.  Several  researchers  have  proposed  ap¬ 
proaches  to  address  the  local  minima  issue  (e.g.,  see  [2], 
[3],  [5],  [14],  [25]).  One  approach  to  address  the  local 
minima  issue  was  provided  by  Koditschek  in  [16]  for 
holonomic  systems  (see  also  [17]  and  [22])  that  is  based 
on  a  special  kind  of  potential  function,  coined  a  naviga¬ 
tion  function,  that  has  a  refined  mathematical  structure 
which  guarantees  a  unique  minimum  exists. 

By  leveraging  from  previous  results  directed  at  clas¬ 
sic  (holonomic)  systems,  more  recent  research  has  fo¬ 
cused  on  the  development  of  potential  function-based 
approaches  for  more  challenging  nonholonomic  systems 
(e.g.,  wheeled  mobile  robots  (WMRs)).  For  example, 
Laumond  et  al.  [18]  used  a  geometric  path  planner  to 
generate  a  collision-free  path  that  ignores  the  nonholo¬ 
nomic  constraints  of  a  WMR,  then  divided  the  geomet¬ 
ric  path  into  smaller  paths  that  satisfy  the  nonholo¬ 
nomic  constraints,  and  then  applied  an  optimization 
routine  to  reduce  the  path  length.  In  [10]  and  [11], 
Guldner  et  al.  use  discontinuous,  sliding  mode  con¬ 
trollers  to  force  the  position  of  a  WMR  to  track  the 
negative  gradient  of  a  potential  function  and  to  force 
the  orientation  to  align  with  the  negative  gradient.  In 
[1],  [15],  and  [21],  continuous  potential  field-based  con¬ 
trollers  are  developed  to  also  ensure  position  tracking  of 
the  negative  gradient  of  a  potential  function,  and  orien¬ 
tation  tracking  of  the  negative  gradient.  More  recently, 
Ge  and  Cui  present  a  new  repulsive  potential  function 
approach  in  [9]  to  address  the  case  when  the  goal  is  non- 
reachable  with  obstacles  nearby  (GNRON).  In  [23]  and 
[24],  Tanner  et  al.  exploit  the  navigation  function  re¬ 
search  of  [22]  along  with  a  dipolar  potential  field  concept 
to  develop  a  navigation  function-based  controller  for  a 
nonholonomic  mobile  manipulator.  Specifically,  the  re¬ 
sults  in  [23]  and  [24]  use  a  discontinuous  controller  to 
track  the  negative  gradient  of  the  navigation  function, 
where  a  nonsmooth  dipolar  potential  field  causes  the 
WMR  to  turn  in  place  at  the  goal  position  to  align  with 
a  desired  orientation. 

In  this  paper,  two  different  methods  are  proposed  to 
achieve  a  navigation  objective  for  a  nonholonomic  sys¬ 
tem.  In  the  first  approach,  a  3-dimensional  (3D) 


navigation-like  function-based  desired  trajectory  is  gen¬ 
erated  that  is  proven  to  ultimately  approach  to  the  goal 
position  and  orientation  that  is  a  unique  minimum  over 
the  WMR  free  configuration  space.  A  continuous  con¬ 
trol  structure  is  then  utilized  that  enables  the  WMR  to 
follow  the  path  and  stop  at  the  goal  position  and  ori¬ 
entation  setpoint  (i.e. ,  the  controller  solves  the  unified 
tracking  and  regulation  problem).  The  unique  aspect 
of  this  approach  is  that  the  WMR  reaches  the  goal  po¬ 
sition  with  a  desired  orientation  and  is  not  required  to 
turn  in  place  as  in  many  of  the  previous  results.  As  de¬ 
scribed  in  [4]  and  [20],  factors  such  as  the  radial  reduc¬ 
tion  phenomena,  the  ability  to  more  effectively  penalize 
the  robot  for  leaving  the  desired  contour,  the  ability  to 
incorporate  invariance  to  the  task  execution  speed,  and 
the  improved  ability  to  achieve  task  coordination  and 
synchronization  provide  motivation  to  encapsulate  the 
desired  trajectory  in  terms  of  the  current  position  and 
orientation.  For  the  on-line  2D  problem,  a  continuous 
controller  is  designed  to  navigate  the  WMR  along  the 
negative  gradient  of  a  navigation  function  to  the  goal 
position.  As  in  many  of  the  previous  results,  the  orien¬ 
tation  for  the  on-line  2D  approach  requires  additional 
development  (e.g.,  a  separate  regulation  controller;  a 
dipolar  potential  field  approach  [23],  [24];  or  a  virtual 
obstacle  [9])  to  align  the  WMR  with  a  desired  orienta¬ 
tion.  Simulation  results  are  provided  to  illustrate  the 
performance  of  the  second  approach. 

2  Kinematic  Model 

The  class  of  nonholonomic  systems  considered  in  this 
paper  can  be  modeled  as  a  kinematic  wheel 

q  =  S(q)v  (1) 

where  q(t),  q(t)  £  R3  are  defined  as 

q  =  [xc  yc  0]T  q  =  xc  yc  6  .  (2) 

In  (1),  the  matrix  S(q)  £  R3x2  is  defined  as  follows 

cosd  0 

S(q)  =  sin  6  0  (3) 

0  1 

and  the  velocity  vector  v(t)  £  R2  is  defined  as 

v  =  [vc  u)c]T  (4) 

with  vc(t),u>c(t)  £  R  denoting  the  linear  and  angular 

velocity  of  the  system.  In  (2),  xc(t),  yc(t),  and  6(t)  £  R 
denote  the  position  and  orientation,  respectively,  xc(t), 
yc(t)  denote  the  Cartesian  components  of  the  linear  ve¬ 
locity,  and  6(t)  £  R  denotes  the  angular  velocity. 


3  Control  Objective 

The  control  objective  in  this  paper  is  to  navigate  a  non¬ 
holonomic  system  (e.g.,  a  wheeled  mobile  robot)  along 
a  collision-free  path  to  a  constant,  goal  position  and 
orientation,  denoted  by  q*  =  [x*  y*  d*]T  £  R3,  in 
an  obstacle  cluttered  environment  with  known  obsta¬ 
cles.  Specifically,  the  objective  is  to  control  the  non¬ 
holonomic  system  along  a  path  from  an  initial  position 
and  orientation  to  q*  £T>,  where  T>  denotes  a  free  con¬ 
figuration  space.  The  free  configuration  space  V  is  a 
subset  of  the  whole  configuration  space  with  all  config¬ 
urations  removed  that  involve  a  collision  with  an  ob¬ 
stacle.  To  quantify  the  path  planning-based  control 
objective,  the  difference  between  the  actual  Cartesian 
position  and  orientation  and  the  desired  position  and 
orientation,  denoted  by  qd(t)  =[xcd(t)  ycd(t)  6d(t)]T, 
is  defined  as  e  (t)  =  [  x  ( t )  y  (f)  9(t)  ] T  £  R3  as  fol¬ 

lows 

x  =  xc-xcd  y  =  yc-ycd  6  =  6-  6d  (5) 
where  the  desired  trajectory  is  designed  so  that  qd(t)  — > 

>k 

q ■ 

Motived  by  the  navigation  function  approach  in  [16],  a 
navigation-like  function  is  utilized  to  generate  the  de¬ 
sired  path  qd{t).  Specifically,  the  navigation- like  func¬ 
tion  used  in  this  paper  is  defined  as  follows 

Definition  1  Let  V  be  a  compact  connected  analytic 
munifold  with  boundary,  and  let  q*  be  a  goal  point  in 
the  interior  ofV.  The  navigation-like  function  p(q)  : 
D  —*[0,  1],  is  a  function  satisfy  the  following  properties: 

1.  p  (q(t))  is  first  order  and  second  order  differen¬ 
tiable  (i.e.,  f^p(q(t)) and  ^p(q(t)^j  exist  on 
V). 

2.  ip  ( q(t ))  obtains  its  maximum  value  on  the  bound¬ 
ary  of  'D. 

3.  ip  ( q(t) )  has  unique  global  minimum  at  q(t)  =  q* . 

4.  If  f^ip  ( q(t ))  <  £z,  then  ||g  (f)  -  q*\\  <  er  with 

ez,£r  €  R  being  known  positive  constants. 

5.  If  ip(q(t))  is  ultimately  bounded  by  e,  then 
|| q(t)  —  (7*||  is  ultimately  bounded  by  £r  with  e 
£  R  being  some  known  positive  constant. 

4  Online  3D  Path  Planner 
4.1  Trajectory  Planning 

The  3D  desired  trajectory  can  be  generated  online  as 
follows 

qd  =  -p  ( q )  V  P  (q)  +  vr  (6) 

p.  2 


where  (p(q)  £  R  denotes  a  navigation- like  function  de¬ 
fined  in  Definition  1,  \/<f(q)  £  R3  denotes  the  gradient 
vector  of  ip  (q),  and  vr  (t)  £  R3  is  an  additional  control 
term  to  be  designed. 

Assumption  The  navigation-like  function  defined  in 
Definition  1  along  with  the  desired  trajectory  gen¬ 
erated  by  (6)  ensures  an  auxiliary  terms  N  (•)  £ 
R3,  defined  as 

N  =  V  (Qd)  V  (Qd)  -  <p  (q)  V  (q)  >  (7) 

satisfy  the  following  inequality 

ll^ll  <  P(Qd,e)  ||e||  (8) 

where  the  positive  function  p(-)  is  nondecreasing 
in  ||9d||  and  ||e||.  The  inequality  given  by  (8)  will 
be  used  in  the  subsequent  stability  analysis. 


4.2  Model  Transformation 

To  achieve  the  control  objective,  a  controller  must  be 
designed  to  track  the  desired  trajectory  developed  in  (6) 
and  stop  at  the  goal  position  q*.  To  this  end,  the  uni¬ 
fied  tracking  and  regulation  controller  presented  in  [7] 
can  be  used.  To  develop  the  controller  in  [7],  the  open- 
loop  error  system  defined  in  (5)  must  be  transformed 
into  a  suitable  form.  Specifically,  the  position  and  ori¬ 
entation  tracking  error  signals  defined  in  (5)  are  related 
to  the  auxiliary  tracking  error  variables  w(t)  £  R  and 

z(t)  =  [  Z\{t)  22(f)  ]T  £  R2  through  the  following 

global  invertible  transformation  [8] 


w 

—9  cos  9  +  2  sin  9  —9  sin  9  —  2  cos  9  0 

X 

Zl 

0  0  1 

y 

z 2 

cos  9  sin  9  0 

9 

(9) 


After  taking  the  time  derivative  of  (9)  and  using  (l)-(5) 
and  (9),  the  tracking  error  dynamics  can  be  expressed  in 
terms  of  the  auxiliary  variables  defined  in  (9)  as  follows 
[8] 

w  =  uTJTz  +  f  (10) 

2  =  u 


where  J  £  R2x2 
fined  as 


denotes  a  skew-symmetric  matrix  de- 


0  -1 
1  0  ’ 


(11) 


and  f(9,  Z2,  qd)  €  R  is  defined  as 

/  =  2[  —  sin#  cos 9  22  \  qd-  (12) 


The  auxiliary  control  input  u(t)  =  [  Ui(t)  u2(t)  ]T  £ 
R2  introduced  in  (10)  is  defined  in  terms  of 
v  (t)  ,9  (t)  ,x(t),y  (t) ,  and  qd  (t )  as  follows 


0  1 
1  —x  sin  0  +  y  cos  0 

'  ed 

id  cos  9  +  ijd  sin  9 


(13) 


4.3  Control  Development 

To  facilitate  the  control  development,  an  auxiliary  error 
signal,  denoted  by  z(t)  £  M2,  is  defined  as  the  difference 
between  the  subsequently  designed  dynamic  oscillator¬ 
like  signal  Zd(t)  £  R2  and  the  transformed  variable  z(t), 
defined  in  (9),  as  follows 

z  =  zd-  z.  (14) 

Based  on  the  open-loop  kinematic  system  given  in  (10) 
and  the  subsequent  stability  analysis,  we  design  u(t)  as 
follows  [7] 

u  =  ua~  k2z  (15) 

where  k2  £  R  is  a  positive,  constant  control  gain.  The 
auxiliary  control  term  ua{t)  £  R2  introduced  in  (15)  is 
defined  as 


kia*  +  Jzd  +  ^ lZd  >  (16) 

sd  J 

where  the  auxiliary  signal  Zd(t)  is  defined  by  the  follow¬ 
ing  differential  equation  and  initial  condition 


Zd 


2j(0)2d(0) 


6d  ,  fhw  +  f  \ 

Tzd  +  - - b  Will  Jzd 

sd  V  si  J 

S2d(  0).  (17) 


The  auxiliary  terms  Oi (w,f,t)  £  R  and  Sd(t)  £  R  are 
defined  as 


Di 


k2 


kiw  +  f\ 

%  J 


(18) 


and 


6d  =  a0exp(-ait)  +  S!  (19) 


respectively,  k-\ ,  cuo,  «i,  £\  £  R  are  positive,  constant 
control  gains,  and  f(9,z2,tjd)  was  defined  in  (12).  As 
described  in  [8],  motivation  for  the  structure  of  (17)  and 
(19)  is  based  on  the  fact  that 


INI2  =  & 


(20) 


Based  on  (9),  e(t)  can  be  expressed  in  terms  of  w(t), 
2  ( t ) ,  and  Zd  (f )  as  follows 


w 

r 

II 

Zl 

+  r2 

Zdl 

_  Zd2  _ 

.  *2  _ 

where  R\  (•)  €  R3x3,i?2(')  €  R3x2  are  defined  as  fol¬ 
lows 


Ri  = 


IcosS  —  \zd2  sin  6  —  ^  (21  sin  #  +  2  cos  0) 

—  ^cos0  ^2^2  cos  f?  ^  (21  cos  9  —  2  sin  9) 

0  -1  0 


(22) 


r2  = 


\zd2  sin# 
-\zd2  cos  9 

1 


cos  9 
sin# 
0 


(23) 


p.  3 


u 


V 


Motivated  by  the  subsequent  stability  analysis,  the  ad¬ 
ditional  control  term  vr  (t)  in  (6)  is  designed  as  follows 

vr  =  -k3pl  v  P  (qd)  -  hpl  V  7>  (Qd)  (24) 

where  k3,k4  £  K.  denotes  positive,  constant  con¬ 
trol  gains,  and  the  positive  functions  px  (zdi,zi,qd,e), 
P-2  izdi ,  zi,  Qd,  e)  £  K.  are  defined  as  follows 

Pi  =  p(qd,e)  ll-Rill  p2  =  p(qd,e)\\R2\\  (25) 

4.4  Closed-loop  Error  System 

After  substituting  (15)  into  (10),  the  dynamics  for  w  (t) 
can  be  obtained  as  follows 

w  =  ulJz-u^Jzd  +  f  (26) 

where  (14)  and  the  properties  of  J  in  (11)  were  utilized. 
After  substituting  (16)  into  (26)  for  only  the  second 
occurrence  of  ua(t),  utilizing  (20)  and  the  properties  of 
J  in  (11),  the  final  expression  for  the  closed- loop  error 
system  for  w(t)  can  be  obtained  as  follows 

w  =  u^Jz^kiw.  (27) 

To  determine  the  closed-loop  error  system  for  z(t),  we 
take  the  time  derivative  of  (14)  and  then  substitute  (10) 
and  (17)  into  the  resulting  expression  to  obtain  the  fol¬ 
lowing  expression 

z=  j-^d  +  ^  ^2+  ~  +  wfl^j  Jzd  -  u.  (28) 

After  substituting  (15)  and  (16)  into  (28),  (28)  can  be 
rewritten  as  follows 

z=  -^zd  +  wfli  Jzd  -  £t\zd  +  k2z.  (29) 
Od 

After  substituting  (18)  into  (29)  for  only  the  second 
occurrence  of  fli  (t)  and  then  canceling  common  terms, 
the  following  expression  can  be  obtained 

z=-k2z  +  wJ  jZd  +  QiZd  _  (30) 

Since  the  bracketed  term  in  (30)  is  equal  to  ua  ( t )  de¬ 
fined  in  (16),  the  final  expression  for  the  closed-loop 
error  system  for  z  (t)  can  be  obtained  as  follows 

z=  —k2z  +  wJua.  (31) 

Remark  1  Based  on  the  fact  that  6d  (t)  of  (19)  expo¬ 
nentially  approaches  an  arbitrarily  small  constant,  the 
potential  singularities  in  (16),  (17),  and  (18)  are  always 
avoided. 


4.5  Stability  Analysis 

Theorem  1  Provided  qd  (0)  £  D,  the  desired  trajec¬ 
tory  generated  by  (6)  along  with  the  additional  control 
term  vr  (t)  designed  in  (24)  ensures  that  qd(t )  £  V  and 
|| qd  (t)  —  (7*||  <  er.  where  er  is  defined  in  Definition  1. 

Proof:  Let  V  (t)  £  K.  denote  the  following  function 

V  =  kV i  +  V2  (32) 

where  feel  is  a  positive  constant,  Vi  (t)  £  K.  denotes 
the  following  function 

Vi  =  ^ w 2  +  ^ zTz  (33) 

and  V2  (qd)  ■  D  —> ►  R  denotes  a  function  as  follows 

V2  ( qd )  =  <p  (qd)  •  (34) 

After  taking  the  time  derivative  of  (33)  and  then  sub¬ 
stituting  (27)  and  (31)  into  the  resulting  expression  and 
cancelling  common  terms,  the  following  expression  can 
be  obtained 

V\  =  —kiiv2  —  k2zr  z.  (35) 

After  taking  the  time  derivative  of  (34)  and  utilizing 
(6),  the  following  expression  can  be  obtained 

V2(qd)  =  (VP(qd))Tqd  (36) 

=  -  llvv?0?d)  II 2  ¥>(<?<*) 

+  (VP  (qd)f  N  +  (v<p  ( qd)f  Vr 

where  N  (•)  is  defined  in  (7).  Based  on  (8),  V2  (t)  can 
be  upper  bounded  as  follows 

V2  <  -  \\VP{qd)\\2  p(qd)  (37) 

+P  (Qd,  e )  ||V<P  (?d)||  ||e||  +  (VV7  (hd))T  vr. 

After  substituting  (21)  into  (37),  the  following  inequal¬ 
ity  can  be  obtained 

V2  <  -|lw(<fc)llM<te)  (38) 

+Pi  (zdi,zi,qd,e)  ||V^(%)II  ll^i  (f)ll 
+p2  (zd2,qd,e)  || S/p  (gd)||  \\zd\\ 

+  (VP  {qd))T  Vr 

where  the  vector  Ti  (t)  £  K3  is  defined  as  follows 

Ti  (t)  =  [  w,  zT  ]T ,  (39) 

and  the  positive  function  Pi(zdi,zi,qd,e)  and 
p-2(zdi,z\,qd,e)  are  defined  in  (25).  After  substi¬ 
tuting  (24)  into  (38),  V2  (t)  can  be  rewritten  as 

follows 

v2  <  -  \\vp(qd)\\2  p(qd)  +  -j^  (w2  +  ||5||2)  +^-  ||zd||2. 

(40) 
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Based  on  (35)  and  (40),  the  time  derivative  of  V  ( t )  in  Proof:  Based  on  (33)  and  (35),  V±  (t)  of  (35)  can  be 
(32)  can  be  upper  bounded  by  the  following  inequality  upper  bounded  as  follows 


V  <  -k\w2  —  k-2  \\z\\2  (41) 

-  Ilw> (qd)\\2<p(qd)  +  y  INI2 

where  the  positive  constant  k\ ,  k2  £  K.  are  defined  as 
follows 


ki  =  kk  1  -  k2  =  kk2  -  ^  • 

Case  1:  If  ||  (<Zd) ||2  ^  £*,  from  the  Property  4  in 

Definition  1,  it  is  clear  that 

|| qd  (t)  -  g*  II  <  £r 

Case  2:  If  || (qd)\\2  -5=  £*,  it  is  clear  from  (32),  (33), 
(34),  and  (41)  that 

V  —  €ZV  +  £f  (42) 

where  ez  =  min{±£;i,  \k2,ez},  and  eF  =  ±  ||^d||2  are 
positive  constants.  Based  on  (42),  V  ( t )  can  be  upper 
bounded  as  follows 

V(t)^V (0)  exp  (~ezt)  +  —  (1  -  exp  (~ezt))  (43) 

therefore 

V(t)^V(  0)  +  — .  (44) 

Based  on  (32),  (34),  and  (44),  it  is  clear  that 

<fi(qd(t))  <  ¥>(%(0))  +  k  (V^O)  +  ^  .  (45) 

If  qd  (0)  is  not  on  the  boundary  of  T>,  <p  (qd  (0))  <  1. 
Then  k  can  be  adjusted  to  ensure 

<^d(0))  +  fc(Vi(0)  +  ^  <1.  (46) 

Based  on  (45)  and  (46),  ip  (qd  (t))  <  1,  hence  qd  (t)  £  V 
from  Definition  1.  It  is  clearly  from  (43)  that  <p(qd)  is 
ultimately  bounded  by  Therefore,  if  ||v<y?(9d)lr  ^ 
ez,  ki  can  be  adjusted  to  ensure  ¥-  =  £,  where  £  is 
defined  in  Definition  1.  Hence  by  the  Property  5  in 
Definition  1,  \\qd  (f)  —  g*||  is  ultimately  bounded  by  er. 
□ 


Theorem  2  The  kinematic  control  law  given  in  (15)- 
(19)  ensures  global  uniformly  ultimately  bounded 
(GUUB)  position  and  orientation  tracking  in  the  sense 
that 


I x(t)\,\y(t)\,  d(t)  <  £2  exp  (~j0t)  +  £3£i  (47) 


where  £\  was  given  in  (19),  £2  — 

w2  (0)  +  z\  (0)  +  z2  (0) ,  and  £3  and  y0  are  posi¬ 
tive  constants. 


Vj  ^  —  2  min  {  k\,  k2  }  V\ .  (48) 

Based  on  (48) ,  the  following  inequality  can  be  obtained 

Vi  (t)  <  exp  (—2  min  {  k\,  k2  }  t )  Vj  (0) .  (49) 

Based  on  (33),  (49)  can  be  rewritten  as  follows 

H^i  (i)||  <  exp  (-min  {  £q,  k2  }  t)  ||'Pi  (0)  ||  (50) 

where  the  vector  Ti  (f)  is  defined  in  (39). 

From  (33)  and  (49),  it  is  clear  that  w(t),z(t )  £ 
Too.  Based  on  (19)  and  (20),  we  can  conclude  that 
Zd(t)  £  £00 •  From  (14)  and  z(t),Zd(t )  £  £00,  it 
is  clear  that  z(t)  £  C^.  Since  w(t),z(t )  £  Coo, 
based  on  the  inverse  transformation  from  (9),  e(t)  £ 
Coo  •  Based  on  qd  (t)  £  Coo  from  Theorem  1  and 
e(t)  £  Coo,  it  is  clear  that  q(t)  £  Coo ■  From  (22)- 
(25),  qd  ( t ) ,  zd  ( t )  ,z(t) ,  e  (t)  £  Coo,  and  the  prop¬ 
erties  in  Definition  1,  we  can  conclude  that  iy  (i), 
qd  (t)  £  Coo ■  Based  on  (12)  and  q(t),z  ( t ) ,  qd  (t)  £  Coo, 
f  (0,z2,qd)  €  Coo ■  Then  Hi  (t)  £  Coo  from  (18).  Then 
u  ( t ) ,  ua  (t) ,  id  ( t )  £  Coo  from  (15)-(17).  Based  on  the 
fact  that  f  (0,z2,qd)  ,z(t)  ,u(t)  £  Coo,  then  (10)  can 
be  used  to  conclude  w  (t)  ,z(t)  £  Coo  ■  It  is  clear  from 
z(t),Zd(t)  £  Coo  that  5  ( t )  £  Coo-  Then  standard 
signal  chasing  arguments  can  be  employed  to  conclude 
that  all  of  the  remaining  signals  in  the  control  and  the 
system  remain  bounded  during  closed-loop  operation. 

Based  on  (19),  (20),  (39),  and  (50),  the  triangle  inequal¬ 
ity  can  be  applied  to  (14)  to  prove  that 

INI  <  \\z\\  +  \\zd\\  (51) 

<  exp  (—  min  {  £q,  k2  }  t)  ||\fr1  (0)| 

+ao  exp(— ait)  +  £1. 

Utilizing  (50)-(51),  the  result  given  in  (47)  can  be  ob¬ 
tained  from  taking  the  inverse  of  the  transformation 
given  in  (9).  □ 


Remark  2  Although  qd  ( t )  is  a  collision-free  path,  the 
stability  result  in  Theorem  2  only  ensures  practical 
tracking  of  the  path  in  the  sense  that  the  actual  WMR 
trajectory  is  only  guaranteed  to  remain  in  a  neighbor¬ 
hood  of  the  desired  path.  From  (5)  and  (47),  the  follow¬ 
ing  bound  can  be  developed 

IMI  <  11% ||  +  73£2  exp  (—7 0t)  +  \/3£3£i  (52) 

where  qd  (t)  £  V  based  on  the  proof  for  Theorem  1.  To 
ensure  that  q(t)  £V,  the  free  configuration  space  needs 
to  be  reduced  to  incorporate  the  effects  of  the  second 
and  third  terms  on  the  right  hand  side  of  (52).  To 


this  end,  the  size  of  the  obstacles  could  be  increased 
by  \/3  (£2  +  £3£i)  ,  where  £3^1  can  be  made  ai'bitrarily 
small  by  adjusting  the  control  gains.  To  minimize  the 
effects  of  e 2,  the  initial  conditions  w( 0)  and  z(0)  (and 
hence,  x  (0) ,  y  (0)  ,0(0))  could  be  required  to  be  suffi¬ 
ciently  small  enough  to  yield  a  feasible  path  to  the  goal. 

5  Online  2D  Navigation 

In  the  previous  approach,  the  size  of  the  obstacles 
is  required  to  be  increased  due  to  the  fact  that  the 
navigation-like  function  is  formulated  in  terms  of  the 
desired  trajectory.  In  the  following  approach,  the  navi¬ 
gation  function  proposed  in  [22]  is  formulated  based  on 
current  position  feedback,  and  hence,  q  ( t )  can  be  proven 
to  be  a  member  of  T>  without  placing  restrictions  on  the 
initial  conditions. 


5.1  Trajectory  Planning 

Let  p(xc,yc)  £  R  denote  a  2D  position-based  navi¬ 
gation  function  defined  in  T>  that  is  generated  online, 
where  the  gradient  vector  of  p  (xc.  yc)  is  defined  as  fol¬ 


lows 


V<P  (xc,yc)  = 


dip  dp  T 
dxc  dyc 


(53) 


Let  0d  (xc,  yc)  £  R  denote  a  desired  orientation  that  is 
defined  as  a  function  of  the  negated  gradient  of  the  2D 
navigation  function  as  follows 


"■‘-‘rc“"2( -S'  “£)  (54) 


where  arctan2(-)  :  R2  — >■  R  denotes  the  four  quadrant 
inverse  tangent  function  [26],  where  0d  (i)  is  confined  to 
the  following  region 


—7 r  <  0d  ^  7r. 


As  stated  in  [21],  by  defining  0d\(x*,y*)  = 

arctan2(0,0)  =  9\(x*,y*),  then  6d(t)  remains  con¬ 
tinuous  along  any  approaching  direction  to  the  goal 
position.  See  Appendix  for  an  expression  for  9d(t) 
based  on  the  previous  continuous  definition  for  0d(t). 


Remark  3  As  discussed  in  [22],  the  construction  of  the 
function  p  ( q(t )),  coined  a  navigation  function,  that  sat¬ 
isfies  the  first  three  properties  in  Definition  1  for  a  gen¬ 
eral  obstacle  avoidance  problem  is  nontrivial.  Indeed, 
for  a  typical  obstacle  avoidance,  it  does  not  seem  pos¬ 
sible  to  construct  p(q(t))  such  that  -§^p(q(t))  =  0  only 
at  q  (t)  =  q* .  That  is,  as  discussed  in  [22],  the  appear¬ 
ance  of  interior  saddle  points  (i.e.,  unstable  equilibria) 
seems  to  be  unavoidable;  however,  these  unstable  equi¬ 
libria  do  not  really  cause  any  difficulty  in  practice.  That 
is,  p(q(t))  can  be  constructed  as  shown  in  [22]  such  that 
only  a  “few”  initial  conditions  will  actually  get  stuck  on 
the  unstable  equilibria. 


5.2  Control  Development 

Based  on  the  open- loop  system  introduced  in  (l)-(4) 
and  the  subsequent  stability  analysis,  the  linear  velocity 
control  input  vc  ( t )  is  designed  as  follows 

vc  =  kv  Hv^ll  cos  0  (55) 

where  kv  £  R  denotes  a  positive,  constant  control  gain, 
and  0(t)  was  introduced  in  (5).  After  substituting  (55) 
into  (1),  the  following  closed-loop  system  can  be  ob¬ 
tained 

xc  1  .  I”  cos 6  1M  „  ~n 

==  kv  .  n  \7w  cost/.  (56) 
yc  sm  0  11  v  tii  v  ’ 

The  open-loop  orientation  tracking  error  system  can  be 
obtained  by  taking  the  time  derivative  of  6(t)  in  (5)  as 
follows 

0=  wc  -  0d  (57) 

where  (1)  was  utilized.  Based  on  (57),  the  angular  ve¬ 
locity  control  input  c oc  ( t )  is  designed  as  follows 

wc  =  -k^O  +  0d  (58) 

where  ku  £  R  denotes  a  positive,  constant  control  gain, 
and  6d(t)  denotes  the  time  derivative  of  the  desired  ori¬ 
entation.  See  Appendix  for  an  explicit  expression  for 
0d  (' t ).  After  substituting  (58)  into  (57),  the  closed-loop 
orientation  tracking  error  system  is  given  by  the  follow¬ 
ing  linear  relationship 

0=  -kj.  (59) 

Linear  analysis  techniques  can  be  used  to  solve  (59)  as 
follows 

0(t)  =  0(0)  exp(— kut).  (60) 

After  substituting  (60)  into  (56)  the  following  closed- 
loop  error  system  can  be  determined 

=  kv  IIVT’II  cos  (b{ 0)  exp (-kj)^  . 

(61) 

5.3  Stability  Analysis 

Theorem  3  The  control  input  designed  in  (55)  and 
(58)  along  with  the  navigation  function  p  (xc  (t) ,  yc  (t)) 
ensure  asymptotic  navigation  in  the  sense  that 

\x  (t)  —  x*  | ,  \y  (t)  —  y* | ,  0(t)  — y  0  as  t  — >  00.  (62) 

Proof:  Let  V3  (xc,  yc)  :  V  — >  R  denote  the  following 
non-negative  function 

V3  (xc,  yc)  =  p  (xc,  yc) .  (63) 

p.  6 


After  taking  the  time  derivative  of  (63)  and  utilizing  (1), 
(53),  and  (56),  the  following  expression  can  be  obtained 


Vz  =  kv  (v^)T 


cos  0 
sin  0 


V<p||  cos  0. 


(64) 


Based  on  the  development  provided  in  Appendix,  the 
gradient  of  the  navigation  function  can  be  expressed  as 
follows 


V<6>  =  -  llv^ll  [  cos0d  sin 0d]T  ■  (65) 

After  substituting  (65)  into  (64),  the  following  expres¬ 
sion  can  be  obtained 


the  objective,  a  desired  orientation  9d  (t)  was  defined 
as  a  function  of  the  negated  gradient  of  the  2D  navi¬ 
gation  function.  The  previous  development  can  be  used 
to  prove  the  result  in  (62).  If  a  navigation  function 
p{xc,yc)  can  be  found  such  that  0d\(x*,y*)  =  0* ,  then 
asymptotic  navigation  can  be  achieved  by  the  controller 
in  (55)  and  (58);  otherwise,  a  standard  regulation  con¬ 
troller  (e.g.,  see  [8]  for  several  candidates)  could  be  im¬ 
plemented  to  regulate  the  orientation  of  the  WMR  from 
9d  I  (x* ,y* )  — »  6*.  Alternatively,  a  dipolar  potential  field 
approach  [23],  [24]  or  a  virtual  obstacle  [9]  could  be  uti¬ 
lized  to  align  the  gradient  field  of  the  navigation  function 
to  the  goal  orientation  of  the  WMR. 


Vs  =  —  kv  Hv^ll2  {cos 9 cos 9d  +  sin 0 sin 0,/)  cos 6k  (66) 


6  Simulation  Results 


After  utilizing  a  trigonometric  identity,  (66)  can  be 
rewritten  as  follows 


=  -git)  (67) 

where  g(t)  £  R  denotes  the  following  positive  function 
g(t)  =  kv  llv^ll2  cos2  9.  (68) 


Based  on  (53)  and  the  property  of  the  navigation  func¬ 
tion  (Similar  to  the  Property  1  of  Definition  1),  it  is 
clear  that  || s/tp  {xc,  yc)||  £  Coo  on  V\  hence,  (55)  can  be 
used  to  conclude  that  vc  ( t )  £  on  D.  Development  is 

also  provided  in  the  Appendix  that  proves  9d  (t)  £  C ^ 
on  V ;  hence,  (58)  can  be  used  to  show  that  uc  ( t )  £  Coo 
on  V.  Based  on  the  fact  that  vc  {t)  £  Coo  on  'D,  (1)- 
(4)  can  be  used  to  prove  that  xc  (t),  yc  ( t )  £  Coo  on  V. 
After  taking  the  time  derivative  of  (53)  the  following 
expression  can  be  obtained 


d_ 

dt 


{\Dp{xc,yc)) 


d2ip  d2p  | 

Xc 
Vc 

dxcdyc  dy)  J 


dx2c  dycdxc 

d2ip  d2ip 


(69) 

Since  xc  (t),  yc  (t)  £  Coo  on  D,  and  since  each  element  of 
the  Hessian  matrix  in  (69)  is  bounded  by  the  property 
of  the  navigation  function  (Similar  to  the  Property  1  of 
Definition  1),  it  is  clear  that  g(t)  £  Coo  on  V.  Based 
on  (63),  (67),  (68),  and  the  fact  that  g(t)  £  Coo  on  V, 
then  Lemma  A. 6  of  [6]  can  be  invoked  to  prove  that 


II VV9  ixc,  Vc)\\2  cos2  0  — >  0  (70) 


To  illustrate  the  performance  of  the  controller  given  in 
(55)  and  (58),  a  numerical  simulation  was  performed 
to  navigate  the  WMR  from  q  (xc  (0) ,  yc  (0) ,  0  (0))  to 
q*  (x*,  y[,9*).  Since  the  properties  of  a  navigation  func¬ 
tion  are  invariant  under  a  diffeomorphism,  a  diffeomor- 
phism  is  developed  to  map  the  WMR  free  configuration 
space  to  a  model  space  [17] .  Specifically,  a  positive  func¬ 
tion  ip  (. xc ,  yc)  was  chosen  as  follows 

^  (x  v  )  — _ (xc  -  x*)  +{Vc-yt) _ 

<P(xoVc)-  1/K. 

[((xc-x*)  +(yc-y*) )  +P0P1) 

(71) 

where  k  is  positive  integer  parameter,  and  the  bound¬ 
ary  function  /30{xc,yc)  £  R  and  the  obstacle  function 
/?i  (. xc ,  yc)  £  M  are  defined  as  follows 

Po  ~  rl-(xc-xrof -(yc-yrof  (72) 
0i  ~  (xc  ^  xri)2  +  (yc  -  yri)2  -  r\. 

In  (72),  (xro,yr 0)  and  {xri,yri)  are  the  centers  of  the 
boundary  and  the  obstacle  respectively,  ro,  r\  £  M  are 
the  radii  of  the  boundary  and  the  obstacle  respectively. 
From  (71)  and  (72),  it  is  clear  that  the  model  space  is  a 
unit  circle  that  excludes  a  circle  described  by  the  obsta¬ 
cle  function  (3l  ( xc,yc )■  If  more  obstacles  are  present, 
the  corresponding  obstacle  functions  can  be  easily  in¬ 
corporated  into  the  navigation  function  [17].  In  [17], 
Koditschek  proved  that  ip  ( xc ,  yc )  in  (71)  is  the  naviga¬ 
tion  function  for  (xc  (t) ,  yc  (6)),  provided  that  k  is  big 
enough.  For  the  simulation,  the  model  space  configura¬ 
tion  is  selected  as  follows 


in  the  region  'D.  Based  on  the  fact  that  cos2  0(6)  — ► 
1  from  (60),  then  (70)  can  be  used  to  prove  that 
HVV3  (xc,  0c)  ||  — 5 ►  0.  Therefore  the  result  in  (62)  can 
be  obtained  based  on  the  analysis  in  Remark  3.  □ 

Remark  4  The  control  development  in  this  section  is 
based  on  a  2D  position  navigation  function.  To  achieve 


Xro  0  Uro  0  Vq  1 

xri  =  0  yri  =  0.1  r  i  =  0.15 
where  the  initial  and  goal  configuration  were  selected  as 
g  (0)  =  [  0.1  0.6  51.6  ]T 
q*  =  [  —0.2  -0.4  -40.1  ]T. 


The  control  inputs  defined  in  (55)  and  (58)  were  uti¬ 
lized  to  drive  the  WMR  to  the  goal  point  along  the 
negated  gradient  angle.  The  control  gains  kv  and  ku 
were  adjusted  to  the  following  values  to  yield  the  best 
performance 

kv  =  0.3  ku  =  17.  (73) 

Once  the  WMR  reached  the  goal  position,  the  regula¬ 
tion  controller  in  [8]  was  implemented  to  regulate  the 
WMR  from  6d\(x*,y*)  - ^  0* ■  The  actual  trajectory  of 
WMR  is  shown  in  Figure  1.  The  outer  circle  in  Figure 
1  depicts  the  outer  boundary  of  the  obstacle  free  space 
and  the  inner  circle  represents  the  boundary  around  an 
obstacle.  The  resulting  position  and  orientation  errors 
for  the  WMR  are  depicted  in  Figure  2,  where  the  rota¬ 
tional  error  shown  in  Figure  2  is  the  error  between  the 
actual  orientation  and  goal  orientation.  The  control  in¬ 
put  velocities  vc(t)  and  uc(t)  defined  in  (55)  and  (58), 
respectively,  are  depicted  in  Figure  3.  Note  that  the  an¬ 
gular  velocity  input  was  artificially  saturated  between 
±90[deg-s-1]. 


Figure  1:  Actual  trajectory  of  the  WMR. 


7  Conclusions 

Two  approaches  are  developed  to  incorporate  naviga¬ 
tion  function  approach  into  different  controllers  for  task 
execution  by  a  WMR  in  the  presence  of  known  obsta¬ 
cles.  The  first  approach  utilizes  a  navigation-like  func¬ 
tion  that  is  based  on  3D  position  and  orientation  in¬ 
formation.  The  navigation-like  function  yields  a  path 
from  an  initial  configuration  inside  the  free  configura¬ 
tion  space  to  a  goal  configuration.  A  differentiable, 
oscillator-based  controller  is  then  used  to  enable  the 
mobile  robot  to  follow  the  path  and  stop  at  the  goal 
position.  Using  this  approach,  a  WMR  was  proven  to 
yield  uniformly  ultimately  bounded  path  following  and 


Figure  2:  Position  and  orientation  errors. 


Figure  3:  Linear  and  angular  velocity  inputs. 


regulation  to  the  goal  point  with  an  arbitrarily  defined 
goal  orientation  (i.e.,  the  WMR  is  not  required  to  spin 
in  place  at  the  goal  position  to  achieve  a  desired  orienta¬ 
tion)  .  A  second  approach  is  developed  that  uses  a  navi¬ 
gation  function  that  is  constructed  using  2D  position  in¬ 
formation.  A  differentiable  controller  is  proposed  based 
on  this  navigation  function.  The  advantage  of  this  ap¬ 
proach  is  that  it  yields  asymptotic  position  convergence; 
however,  the  WMR  cannot  stop  at  an  arbitrary  orien¬ 
tation  without  additional  development.  Simulation  re¬ 
sults  are  provided  to  illustrate  the  performance  of  the 
second  approach. 
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Appendix 


Based  on  the  definition  of  9d  ( t )  in  (54),  6d  ( t )  can  be  ex¬ 
pressed  in  terms  of  the  natural  logarithm  as  follows  [26] 
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where  i=y/—l.  After  exploiting  the  following  identities  [26] 
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After  substituting  (1),  (T9),  and  (80)  into  (78),  the  following  ex¬ 
pression  can  be  obtained 
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cos  9 
sin  6 


After  substituting  (55)  and  (77)  into  (81),  the  following  expression 
can  be  obtained 

ed  =  kv  cos  (e)  [  sin  (0d)  -  cos  ( 9d )  ]  (82) 

d2p  d2p 


dx 2  dycdxc 

d2p  d2p 


cos  0 
sin  6 
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By  part  1  of  Definition  1,  each  element  of  the  Hessian  matrix 
is  bounded;  hence,  from  (82),  it  is  straightforward  that  9d(t)  £ 

Coo. 


and  then  utilizing  (74)  the  following  expressions  can  be  ob¬ 
tained 
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After  utilizing  (75)  and  (76),  the  following  expression  can 
be  obtained 
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Based  on  the  expression  in  (74),  the  time  derivative  of  9d  ( t ) 
can  be  written  as  follows 


'  d9a 

d9d  ' 

xc 

dxc 

dyc 

.  y = 

(78) 


p.  10 


